#include "qe_log.h"
#include "qe_bitops.h"
#include "qe_assert.h"
#include "qe_memory.h"
#include "qe_driver.h"
#include "libfdt.h"



QELOG_DOMAIN("ps7-qspi");



#define PS7_QSPI_BASE          0xE000D000
#define PS7_QSPI_FREQ          200000000

/* ps7 qspi register bit masks PS7_QSPI_<REG>_<BIT>_MASK */
#define PS7_QSPI_CR_IFMODE_MASK	        QE_BIT(31)	/* Flash intrface mode*/
#define PS7_QSPI_CR_MSA_MASK		    QE_BIT(15)	/* Manual start enb */
#define PS7_QSPI_CR_MCS_MASK		    QE_BIT(14)	/* Manual chip select */
#define PS7_QSPI_CR_PCS_MASK		    QE_BIT(10)	/* Peri chip select */
#define PS7_QSPI_CR_FW_MASK		        QE_GENMASK(7, 6)	/* FIFO width */
#define PS7_QSPI_CR_SS_MASK		        QE_GENMASK(13, 10)	/* Slave Select */
#define PS7_QSPI_CR_BAUD_MASK		    QE_GENMASK(5, 3)	/* Baud rate div */
#define PS7_QSPI_CR_CPHA_MASK		    QE_BIT(2)	/* Clock phase */
#define PS7_QSPI_CR_CPOL_MASK		    QE_BIT(1)	/* Clock polarity */
#define PS7_QSPI_CR_MSTREN_MASK	        QE_BIT(0)	/* Mode select */
#define PS7_QSPI_IXR_RXNEMPTY_MASK	    QE_BIT(4)	/* RX_FIFO_not_empty */
#define PS7_QSPI_IXR_TXOW_MASK		    QE_BIT(2)	/* TX_FIFO_not_full */
#define PS7_QSPI_IXR_ALL_MASK		    QE_GENMASK(6, 0)	/* All IXR bits */
#define PS7_QSPI_ENR_SPI_EN_MASK	    QE_BIT(0)	/* SPI Enable */
#define PS7_QSPI_LQSPICFG_LQMODE_MASK	QE_BIT(31) /* Linear QSPI Mode */

/* ps7 qspi Transmit Data Register */
#define PS7_QSPI_TXD_00_00_OFFSET	    0x1C	/* Transmit 4-byte inst */
#define PS7_QSPI_TXD_00_01_OFFSET	    0x80	/* Transmit 1-byte inst */
#define PS7_QSPI_TXD_00_10_OFFSET	    0x84	/* Transmit 2-byte inst */
#define PS7_QSPI_TXD_00_11_OFFSET	    0x88	/* Transmit 3-byte inst */

#define PS7_QSPI_TXFIFO_THRESHOLD	    1	/* Tx FIFO threshold level*/
#define PS7_QSPI_RXFIFO_THRESHOLD	    32	/* Rx FIFO threshold level */

#define PS7_QSPI_CR_BAUD_MAX		    0x07	/* Baud rate divisor max val */
#define PS7_QSPI_CR_BAUD_SHIFT		    3	    /* Baud rate divisor shift */
#define PS7_QSPI_CR_SS_SHIFT		    10	    /* Slave select shift */

#define PS7_QSPI_FIFO_DEPTH		        63



/* ps7 qspi register set */
typedef struct {
	volatile qe_u32 cr;		    /* 0x00 */
	volatile qe_u32 isr;	    /* 0x04 */
	volatile qe_u32 ier;	    /* 0x08 */
	volatile qe_u32 idr;	    /* 0x0C */
	volatile qe_u32 imr;	    /* 0x10 */
	volatile qe_u32 enr;	    /* 0x14 */
	volatile qe_u32 dr;		    /* 0x18 */
	volatile qe_u32 txd0r;	    /* 0x1C */
	volatile qe_u32 drxr;	    /* 0x20 */
	volatile qe_u32 sicr;	    /* 0x24 */
	volatile qe_u32 txftr;	    /* 0x28 */
	volatile qe_u32 rxftr;	    /* 0x2C */
	volatile qe_u32 gpior;	    /* 0x30 */
	volatile qe_u32 reserved0[19];
	volatile qe_u32 txd1r;	    /* 0x80 */
	volatile qe_u32 txd2r;	    /* 0x84 */
	volatile qe_u32 txd3r;	    /* 0x88 */
	volatile qe_u32 reserved1[5];
	volatile qe_u32 lqspicfg;	/* 0xA0 */
	volatile qe_u32 lqspists;	/* 0xA4 */
} ps7_qspi_regs;

typedef struct
{
    qe_spi_bus bus;
	qe_spi_dev dev;
    ps7_qspi_regs *regs;
    qe_u32 fifo_depth;
    qe_u32 cs;
} ps7_qspi_dev;

static ps7_qspi_dev qspi = {
    .regs = (ps7_qspi_regs *)PS7_QSPI_BASE,
    .fifo_depth = 63,
    .cs = 0,
};

typedef struct {
    qe_u8 *txp;
    qe_u8 *rxp;
    qe_u32 tx_bytes;
    qe_u32 rx_bytes;
} ps7_qspi_transfer;

static qe_spi_bus qspi0_bus;
static qe_spi_dev qspi0_device0;



//static qe_u32 ps7_qspi_reg_read(ps7_qspi_dev *dev, qe_u32 offset)
//{
//    qe_u32 base = (qe_u32)dev->regs;
//    return Xil_In32(base + offset);
//}

static void ps7_qspi_reg_write(ps7_qspi_dev *dev, qe_u32 offset, qe_u32 val)
{
    qe_u32 base = (qe_u32)dev->regs;
    volatile qe_u32 *p = (volatile qe_u32 *)(base + offset);
    *p = val; 
}

static qe_ret ps7_qspi_configure(qe_spi_dev *device, qe_spi_configure *cfg)
{
    return qe_ok;
}

static void ps7_qspi_cs_assert(ps7_qspi_dev *dev)
{
    qe_u32 cfg;
    ps7_qspi_regs *regs = dev->regs;
    cfg = regs->cr;
    cfg &= ~PS7_QSPI_CR_SS_MASK;
    cfg |= (~(1 << dev->cs) << PS7_QSPI_CR_SS_SHIFT) &
                PS7_QSPI_CR_SS_MASK;
    regs->cr = cfg;
}

static void ps7_qspi_cs_deassert(ps7_qspi_dev *dev)
{
    ps7_qspi_regs *regs = dev->regs;
    regs->cr |= PS7_QSPI_CR_SS_MASK;
}

static void ps7_qspi_write_data(ps7_qspi_dev *dev, qe_u32 *data, qe_u8 size,
    ps7_qspi_transfer *transfer)
{
    if (transfer->txp) {
        switch (size) {
        case 1:
            *data = *((qe_u8 *)transfer->txp);
            transfer->txp += 1;
            *data |= 0xFFFFFF00;
            break;
		case 2:
			*data = *((qe_u16 *)transfer->txp);
			transfer->txp += 2;
			*data |= 0xFFFF0000;
			break;
		case 3:
			*data = *((qe_u16 *)transfer->txp);
			transfer->txp += 2;
			*data |= (*((qe_u8 *)transfer->txp) << 16);
			transfer->txp += 1;
			*data |= 0xFF000000;
			break;
		case 4:
			/* Can not assume word aligned buffer */
			qe_memcpy(data, transfer->txp, size);
			transfer->txp += 4;
			break;
		default:
			/* This will never execute */
			break;
        }
    } else {
		*data = 0;
	}

    //qspi_debug("data 0x%08x txp %p size %d", *data, transfer->txp, size);

	transfer->tx_bytes -= size;
	if (transfer->tx_bytes < 0)
		transfer->tx_bytes = 0;
}

static void ps7_qspi_fill_tx_fifo(ps7_qspi_dev *dev, ps7_qspi_transfer *transfer,
    qe_uint size)
{
    qe_u32 data;
    qe_u32 fifo_count = 0;
    qe_u32 len, offset;
    ps7_qspi_regs *regs = dev->regs;
	static const unsigned offsets[4] = {
		PS7_QSPI_TXD_00_00_OFFSET, PS7_QSPI_TXD_00_01_OFFSET,
		PS7_QSPI_TXD_00_10_OFFSET, PS7_QSPI_TXD_00_11_OFFSET};

    while ((fifo_count < size) &&
           (transfer->tx_bytes > 0)) {
        
        if (transfer->tx_bytes >= 4) {
            if (transfer->txp) {
                qe_memcpy(&data, transfer->txp, 4);
                transfer->txp += 4;
            } else {
                data = 0;
            }
            regs->txd0r = data;
            transfer->tx_bytes -= 4;
            fifo_count++;
        } else {
            /* Write TXD1, TXD2, TXD3 only if TxFIFO is empty. */
            if (!(regs->isr & PS7_QSPI_IXR_TXOW_MASK) &&
                !transfer->rxp)
                return;
            //qspi_debug("write data");
            len = transfer->tx_bytes;
            ps7_qspi_write_data(dev, &data, len, transfer);
            offset = (transfer->rxp) ? offsets[0] : offsets[len];
            //qspi_debug("offset %d", offset);
            ps7_qspi_reg_write(dev, offset, data);
        }
    }
}

static void ps7_qspi_read_data(ps7_qspi_dev *dev, qe_u32 data, qe_u8 size, 
    ps7_qspi_transfer *transfer)
{
	qe_u8 byte3;

	// qspi_debug("data 0x%04x rx_buf addr: 0x%08x size %d",
	//       data, (unsigned)(transfer->rxp), size);

	if (transfer->rxp) {
		switch (size) {
		case 1:
			*((qe_u8 *)transfer->rxp) = data;
			transfer->rxp += 1;
			break;
		case 2:
			*((qe_u16 *)transfer->rxp) = data;
			transfer->rxp += 2;
			break;
		case 3:
			*((qe_u16 *)transfer->rxp) = data;
			transfer->rxp += 2;
			byte3 = (qe_u8)(data >> 16);
			*((qe_u8 *)transfer->rxp) = byte3;
			transfer->rxp += 1;
			break;
		case 4:
			/* Can not assume word aligned buffer */
			qe_memcpy(transfer->rxp, &data, size);
			transfer->rxp += 4;
			break;
		default:
			/* This will never execute */
			break;
		}
	}
	transfer->rx_bytes -= size;
	if (transfer->rx_bytes < 0)
		transfer->rx_bytes = 0;
}

static int ps7_qspi_irq_poll(ps7_qspi_dev *dev, ps7_qspi_transfer *transfer)
{
    qe_u32 isr;
    qe_u32 rxindex = 0;
    qe_u32 rxcount;
    qe_u32 data;
    ps7_qspi_regs *regs = dev->regs;

    while (!regs->isr);

    isr = regs->isr;
    regs->isr = isr;

    /* Disable all interrupts */
    regs->idr = PS7_QSPI_IXR_ALL_MASK;
    if ((isr & PS7_QSPI_IXR_TXOW_MASK) ||
        (isr & PS7_QSPI_IXR_RXNEMPTY_MASK)) {
        
		/*
		 * This bit is set when Tx FIFO has < THRESHOLD entries. We have
		 * the THRESHOLD value set to 1, so this bit indicates Tx FIFO
		 * is empty
		 */
        rxcount = transfer->rx_bytes - transfer->tx_bytes;
        rxcount = (rxcount % 4) ? ((rxcount/4)+1) : (rxcount/4);
        //qspi_debug("rxcount:%d", rxcount);
		while ((rxindex < rxcount) &&
				(rxindex < PS7_QSPI_RXFIFO_THRESHOLD)) {
            /* Read out the data from the RX FIFO */
            data = regs->drxr;
            //qspi_debug("read 0x%x", data);
            if (transfer->rx_bytes >= 4) {
                if (transfer->rxp) {
					qe_memcpy(transfer->rxp, &data, 4);
					transfer->rxp += 4;
                }
                transfer->rx_bytes -= 4;
            } else {
				ps7_qspi_read_data(dev, data,
						    transfer->rx_bytes, transfer);
			}
            rxindex++;
            //qspi_debug("rxindex:%d", rxindex);
        }

		if (transfer->tx_bytes) {
			/* There is more data to send */
			ps7_qspi_fill_tx_fifo(dev, transfer,
					       PS7_QSPI_RXFIFO_THRESHOLD);
            regs->ier = PS7_QSPI_IXR_ALL_MASK;
		} else {
			/*
			 * If transfer and receive is completed then only send
			 * complete signal
			 */
			if (!transfer->rx_bytes) {
				/* return operation complete */
                regs->idr = PS7_QSPI_IXR_ALL_MASK;
				return 1;
			}
		}
    }

    return 0;
}

static qe_u32 ps7_qspi_xfer(qe_spi_dev *dev, qe_spi_message *message)
{
    qe_u32 data = 0;
    ps7_qspi_dev *spi = (ps7_qspi_dev *)dev->parent.priv;
    ps7_qspi_regs *regs = spi->regs;
    ps7_qspi_transfer transfer;

    qe_assert(spi != QE_NULL);
    qe_assert(message != QE_NULL);

    transfer.rxp      = (qe_u8 *)message->recv_buf;
    transfer.txp      = (qe_u8 *)message->send_buf;
    transfer.rx_bytes = message->length;
    transfer.tx_bytes = message->length;

    if (message->send_buf) {
        qe_debug("txbuf %d", message->length);
        qe_hexdump_debug(message->send_buf, message->length);
    }

    /* cs take */
    if (message->cs_take) {
        qe_debug("cs take");
        ps7_qspi_cs_assert(spi);
    }

    regs->enr = PS7_QSPI_ENR_SPI_EN_MASK;

    while (transfer.tx_bytes || transfer.rx_bytes) {

        if (transfer.tx_bytes < 4)
            ps7_qspi_fill_tx_fifo(spi, &transfer, transfer.tx_bytes);
        else
            ps7_qspi_fill_tx_fifo(spi, &transfer, spi->fifo_depth);
        regs->ier = PS7_QSPI_IXR_ALL_MASK;

        /* wait for completion */
        do {
            data = ps7_qspi_irq_poll(spi, &transfer);
        } while (data == 0);
    }

    /* cs release */
    if (message->cs_release) {
        ps7_qspi_cs_deassert(spi);
        qe_debug("cs release");
    }

    regs->enr &= ~PS7_QSPI_ENR_SPI_EN_MASK;

    if (message->recv_buf) {
        qe_debug("rxbuf %d", message->length);
        qe_hexdump_debug(message->recv_buf, message->length);
    }

    return message->length;
}

static qe_spi_ops ps7_qspi_ops = {
    .configure = ps7_qspi_configure,
    .xfer = ps7_qspi_xfer,
};

static void ps7_qspi_set_clk_prescaler(ps7_qspi_dev *dev, qe_u8 prescaler)
{
    ps7_qspi_regs *regs = dev->regs;

    regs->cr &= ~PS7_QSPI_CR_BAUD_MASK;
    regs->cr |= (prescaler & PS7_QSPI_CR_BAUD_MAX) <<
			    PS7_QSPI_CR_BAUD_SHIFT;
}

static void ps7_qspi_set_slave(ps7_qspi_dev *dev)
{
    ps7_qspi_regs *regs = dev->regs;
    regs->cr &= ~PS7_QSPI_CR_PCS_MASK;
}

static void ps7_qspi_hw_init(ps7_qspi_dev *dev)
{
    qe_u32 val;
    qe_u32 cfg;
    ps7_qspi_regs *regs = dev->regs;

    /* Disable QSPI */
    regs->enr = ~PS7_QSPI_ENR_SPI_EN_MASK;

	/* Disable Interrupts */
	regs->idr = PS7_QSPI_IXR_ALL_MASK;

	/* Clear the TX and RX threshold reg */
	regs->txftr = PS7_QSPI_TXFIFO_THRESHOLD;
	regs->rxftr = PS7_QSPI_RXFIFO_THRESHOLD;

	/* Clear the RX FIFO */
	while (regs->isr & PS7_QSPI_IXR_RXNEMPTY_MASK)
        ;

	/* Clear Interrupts */
	regs->isr = PS7_QSPI_IXR_ALL_MASK;

	/* Manual slave select and Auto start */
    regs->cr = 0;
	regs->cr &= ~PS7_QSPI_CR_MSA_MASK;
    regs->cr &= ~(PS7_QSPI_CR_CPHA_MASK | PS7_QSPI_CR_CPOL_MASK);
	regs->cr |= PS7_QSPI_CR_IFMODE_MASK | 
           PS7_QSPI_CR_MCS_MASK |
           PS7_QSPI_CR_PCS_MASK |
           PS7_QSPI_CR_FW_MASK |
		   PS7_QSPI_CR_MSTREN_MASK;
    regs->cr &= ~PS7_QSPI_CR_BAUD_MASK;
    regs->cr |= (2 << PS7_QSPI_CR_BAUD_SHIFT);

	/* Disable the LQSPI feature */
	regs->lqspicfg &= ~PS7_QSPI_LQSPICFG_LQMODE_MASK;

    ps7_qspi_set_clk_prescaler(dev, 2);

    ps7_qspi_set_slave(dev);
}

static qe_ret ps7_qspi_probe(const void *fdt, int offset)
{
    int len;
    const char *name;
    const char *dev_name;
    const qe_u32 *prop_reg;
    const qe_u32 *prop_clk;
    ps7_qspi_dev *dev;

    name = fdt_get_name(fdt, offset, QE_NULL);

    prop_reg = (const qe_u32 *)fdt_getprop(fdt, offset, "reg", &len);
    if (!prop_reg) {
        qe_error("%s no reg prop", name);
        return qe_err_param;
    }

	dev_name = fdt_getprop(fdt, offset, "dev", &len);
    if (!dev_name) {
        qe_error("%s no dev prop", name);
        return qe_err_param;
    }

    dev = qe_malloc(sizeof(ps7_qspi_dev));
    qe_assert(dev);

    dev->regs = (ps7_qspi_regs *)(qe_ubase)fdt32_to_cpu(*prop_reg);
    dev->fifo_depth = PS7_QSPI_FIFO_DEPTH;
    dev->cs = 0;

    ps7_qspi_hw_init(dev);

	/* register spi bus */
	qe_spi_bus_register(&dev->bus, name, &ps7_qspi_ops);
	qe_debug("register spi bus %s", name);

	/* attach spi device to spi bus */
	qe_spi_bus_attach_device(&dev->dev, dev_name, name, dev);
	qe_debug("register spi dev %s", dev_name);

    return qe_ok;
}

static const qe_device_id ps7_qspi_ids[] = {
    {.compatible = "xlnx,zynq-qspi",},
    {}
};

QE_DRIVER(ps7_qspi) = {
    .name = "ps7-qspi",
    .of_match = ps7_qspi_ids,
    .probe = ps7_qspi_probe,
};

QE_DRIVER_FORCE_EXPORT(ps7_qspi);
